#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S4,     compass,                  sensorI2CHiTechnicCompass)
#pragma config(Motor,  motorA,          lmotorA,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  motorB,          lmotorB,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  motorC,          loader,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_1,     dmotorA,       tmotorNormal, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     dmotorB,       tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     combine,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_2,     basketmo,      tmotorNormal, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    aim,                  tServoNormal)
#pragma config(Servo,  srvo_S1_C3_2,    ramp,                 tServoNormal)
#pragma config(Servo,  srvo_S1_C3_3,    flpusher,             tServoNormal)
#pragma config(Servo,  srvo_S1_C3_4,    rlpusher,             tServoNormal)
#pragma config(Servo,  srvo_S1_C3_5,    basket,               tServoNormal)
#pragma config(Servo,  srvo_S1_C3_6,    ,                     tServoNormal)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//JoystickDreiver.c is the magic behind the getJoytickSettings(), and a few other functions;
#include "JoystickDriver.c"
#include "HTMC-driver.h"

/*if(spunup == false)
	{
	  if(LLAUNCHER_KEY)
	    buttonWasPressed = true;
	  else if(!LLAUNCHER_KEY && buttonWasPressed)
	  {
	    loaderMotor(100);
	    timer = nPgmTime;
	    buttonWasPressed = false;
	    spunup = true;
	  }
  }
  else if(LLAUNCHER_KEY)
	  buttonWasPressed = true;
	else if(!LLAUNCHER_KEY && buttonWasPressed)
	  {
	    launchMotors(0,0);
	    loaderMotor(0);
	    buttonWasPressed = false;
	    spunup = false;
	  }*/

//Keymap
//Joystick 1
#define RAMP_KEY joy1Btn(5)
#define COMBINE_KEY joy1Btn(6)
#define AIM_UP_KEY joy1Btn(8)
#define AIM_DOWN_KEY joy1Btn(7)


//Joystick 2
#define BASKETWEAKSERVO_UP_KEY joy2Btn(6)
#define BASKETWEAKSERVO_DOWN_KEY joy2Btn(8)
#define BASKETSERVO_UP_KEY joy2Btn(3)
#define BASKETSERVO_DOWN_KEY joy2Btn(1)
#define LOADER_REVERSE_KEY joy2Btn(4)
#define FLPUSHER_KEY joy2Btn(5)
#define LOADER_KEY joy2Btn(2)
#define RLPUSHER_KEY joy2Btn(7)



//Other misc. definitions
#define TELEOP_TIME 900
#define CONTROLLER_DEAD_ZONE 20

//Declare Functions For Project
//Init is where we'll put anything that needs to be done before the round starts. EX Put servos in a certain position,
//etc.
void init();

//drive is for the teleop commands
void drive();

void calibrate();

// *Motors() is shorthand for motor[lmotorA] = ... etc
void launchMotors(short m1, short m2);
void driveMotors(short m1, short m2);
void combineMotor(short m);
void loaderMotor(short m);
void basketMotor(short m);

void init()
{
  servo[ramp] = 160;
  servo[aim] = 38;
  servo[flpusher] = 90;
  servo[rlpusher] = 166;
}

task main()
{
  init();
  waitForStart();
  //calibrate();
  drive();
}

void drive()
{
  //Variables for toggle switches
  bool combineWasPressed = false;
  bool combineSpunup = false;
  bool basketupWasPressed = false;
  bool basketdownWasPressed = false;
  bool rampWasPressed = false;
  bool aimUpWasPressed = false;
  bool aimDownWasPressed = false;
  bool rampSpunup = false;
  int encoderA, encoderB;
  int basketServoPosition = servo[basket];
  int aimPosition = servo[aim];
  int derpderp;

  ClearTimer(T1);
  launchMotors(100,100);

  while(/*time100[T1] <= TELEOP_TIME*/1)
  {
    encoderA = nMotorEncoder[dmotorA];
    encoderB = nMotorEncoder[dmotorB];
  getJoystickSettings(joystick);
  derpderp = SensorValue[compass];

	//Launcher/Loader Controls
	if(LOADER_KEY || LOADER_REVERSE_KEY)
	{
  if(LOADER_KEY)
	  loaderMotor(100);
	if(LOADER_REVERSE_KEY)
	  loaderMotor(-100);
  }
  else
    loaderMotor(0);

	//Combine Controls
	if(combineSpunup == false)
	{
	  if(COMBINE_KEY)
	    combineWasPressed = true;
	  else if(!COMBINE_KEY && combineWasPressed)
	  {
	    combineMotor(100);
	    combineWasPressed = false;
	    combineSpunup = true;
	  }
    }
    else if(COMBINE_KEY)
	  combineWasPressed = true;
	else if(!COMBINE_KEY && combineWasPressed)
	{
	  combineMotor(0);
	  combineWasPressed = false;
	  combineSpunup = false;
	}

	//Basket Motor
	if(joystick.joy2_y1 > 0)
	  basketMotor(joystick.joy2_y1/4);
	else
	  basketMotor(joystick.joy2_y1/6);

  //Basket Servo
	  if(BASKETWEAKSERVO_UP_KEY)
	    basketupWasPressed = true;
	  else if(!BASKETWEAKSERVO_UP_KEY && basketupWasPressed)
	  {
	    basketServoPosition += 2;
	    basketupWasPressed = false;
	  }

	  if(BASKETWEAKSERVO_DOWN_KEY)
	    basketdownWasPressed = true;
	  else if(!BASKETWEAKSERVO_DOWN_KEY && basketdownWasPressed)
	  {
	    basketServoPosition -= 2;
	    basketdownWasPressed = false;
	  }

	  if(BASKETSERVO_UP_KEY)
      basketServoPosition = 134;
    else if(BASKETSERVO_DOWN_KEY)
      basketServoPosition = 86;

    servo[basket] = basketServoPosition;


  //Aim
  aimPosition = servo[aim];
  	if(AIM_UP_KEY)
	  aimUpWasPressed = true;
	  else if(!AIM_UP_KEY && aimUpWasPressed)
	  {
	    aimPosition += 2;
	    aimUpWasPressed = false;
	}
	  	if(AIM_DOWN_KEY)
	  aimDownWasPressed = true;
	  else if(!AIM_DOWN_KEY && aimDownWasPressed)
	  {
	    aimPosition -= 2;
	    aimDownWasPressed = false;
	  }
	servo[aim] = aimPosition;


  //Ramp Controls
	if(rampSpunup == false)
	{
	  if(RAMP_KEY)
	    rampWasPressed = true;
	  else if(!RAMP_KEY && rampWasPressed)
	  {
	    servo[ramp] = 54;
	    rampWasPressed = false;
	    rampSpunup = true;
	  }
  }
  else if(RAMP_KEY)
	  rampWasPressed = true;
	else if(!RAMP_KEY && rampWasPressed)
	{
	  servo[ramp] = 160;
	  rampWasPressed = false;
	  rampSpunup = false;
	}

	//Aim Controls



	//Front T-Pusher Controls
	if(FLPUSHER_KEY)
	   servo[flpusher] = 230;
	else
	   servo[flpusher] = 90;

	 //Rear T-Pusher Controls
	if(RLPUSHER_KEY)
		servo[rlpusher] = 100;
	else
		servo[rlpusher] = 166;

	//Vroom Vroom
	driveMotors(joystick.joy1_y1, joystick.joy1_y2,);
  }
}

void launchMotors(short m1, short m2)
{
  motor[lmotorA] = m1;
  motor[lmotorB] = m2;
}

//Note that driveMotors will ignore values < CONTROLLER_DEAD_ZONE
void driveMotors(short m1, short m2)
{
  //Here we scale the joystick control values from (-127,127) to (-100,100) which is what the controller accepts.
  //There is also a deadzone of 10 either way to keep it stable. Due to an odd hardware glitch, we also have to
  //keep the motors at 1 power in order to keep them from misfiring
  if(abs(m1) >= CONTROLLER_DEAD_ZONE)
    motor[dmotorA] = m1/1.27;
  else
    motor[dmotorA] = 1;
  if(abs(m2) >= CONTROLLER_DEAD_ZONE)
    motor[dmotorB] = m2/1.27;
  else
    motor[dmotorB] = 1;
}

void calibrate()
{
  HTMCstartCal(compass);
  wait10Msec(220;
  HTMCstopCal(compass);

}

void combineMotor(short m)
{
  motor[combine] = m;
}

void loaderMotor(short m)
{
  motor[loader] = m;
}

void basketMotor(short m)
{
  motor[basketmo] = m;
}
